#include "arm_control/arm_action_type3.hpp"
#include <chrono>
#include <thread>

//左右机械臂收获动作

namespace arm_control
{

ArmActionType3::ArmActionType3(rclcpp::Logger logger, rclcpp::Node::SharedPtr node)
  : ArmActionBase(logger), node_(node)
{
  // 创建爪子发布器
  zhuazi_left_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_left", 10);
  zhuazi_right_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/zhuazi_right", 10);
  
  // 初始化MoveGroupInterface，分别为左右机械臂
  move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_left");
  move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, "arm_right");
  
  RCLCPP_INFO(logger_, "动作类型3初始化完成，创建了爪子发布器");
  RCLCPP_INFO(logger_, "MoveIt接口已初始化，Planning Group: arm_left 和 arm_right");
}

void ArmActionType3::execute(
  const std::shared_ptr<GoalHandleARM> goal_handle,
  std::shared_ptr<ARM::Feedback> feedback,
  std::shared_ptr<ARM::Result> result)
{
  RCLCPP_INFO(logger_, "执行动作类型3 - 机械臂放置物体");
  
  // ========== 左臂操作流程 ==========
  
  // 0. 先将yuntai2运动到0.00
  feedback->progress_percentage = 5.0;
  feedback->current_status = "将yuntai2移动到初始位置...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "将yuntai2设置为0.00");
  
  move_group_left_->setStartStateToCurrentState();
  
  // 获取当前状态并修改yuntai2关节
  moveit::core::RobotStatePtr current_state = move_group_left_->getCurrentState(10.0);
  if (!current_state) {
    RCLCPP_ERROR(logger_, "无法获取当前机器人状态");
    result->success = false;
    result->message = "动作类型3失败 - 无法获取当前状态";
    goal_handle->abort(result);
    return;
  }
  
  // 获取所有关节的当前值
  std::vector<double> joint_group_positions;
  const moveit::core::JointModelGroup* joint_model_group = 
    current_state->getJointModelGroup("arm_left");
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
  
  // 只修改yuntai2的值
  const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
  for (size_t i = 0; i < joint_names.size(); i++) {
    if (joint_names[i] == "yuntai2") {
      joint_group_positions[i] = 0.00;
      RCLCPP_INFO(logger_, "找到yuntai2关节，设置为0.00");
      break;
    }
  }
  
  move_group_left_->setJointValueTarget(joint_group_positions);
  
  moveit::planning_interface::MoveGroupInterface::Plan yuntai2_plan;
  bool yuntai2_success = (move_group_left_->plan(yuntai2_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!yuntai2_success) {
    result->success = false;
    result->message = "动作类型3失败 - yuntai2路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "yuntai2路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "yuntai2路径规划成功");
  
  auto yuntai2_execute_result = move_group_left_->execute(yuntai2_plan);
  if (yuntai2_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型3失败 - yuntai2运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "yuntai2运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "yuntai2已到达初始位置");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型3已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型3已取消");
    return;
  }
  
  // 1. 左臂运动到left_gain_pose
  feedback->progress_percentage = 10.0;
  feedback->current_status = "左臂移动到left_gain_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置左臂目标姿态为left_gain_pose");
  
  move_group_left_->setNamedTarget("left_gain_pose");
  move_group_left_->setStartStateToCurrentState();
  
  moveit::planning_interface::MoveGroupInterface::Plan left_gain_plan;
  bool left_gain_success = (move_group_left_->plan(left_gain_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!left_gain_success) {
    result->success = false;
    result->message = "动作类型3失败 - 左臂到left_gain_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到left_gain_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂到left_gain_pose路径规划成功");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型3已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型3已取消");
    return;
  }
  
  feedback->progress_percentage = 20.0;
  feedback->current_status = "左臂正在移动...";
  goal_handle->publish_feedback(feedback);
  
  auto left_gain_execute_result = move_group_left_->execute(left_gain_plan);
  if (left_gain_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型3失败 - 左臂到left_gain_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到left_gain_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂已到达left_gain_pose");
  
  // 延时1秒
  RCLCPP_INFO(logger_, "到达left_gain_pose后延时1秒...");
  std::this_thread::sleep_for(std::chrono::seconds(1));
  
  // 2. 发布/zhuazi_left为120
  feedback->progress_percentage = 30.0;
  feedback->current_status = "打开左爪子...";
  goal_handle->publish_feedback(feedback);
  
  auto left_msg = std_msgs::msg::Int32();
  left_msg.data = 120;
  zhuazi_left_publisher_->publish(left_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_left: 120 (打开)");
  
  // 等待爪子动作完成
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型3已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型3已取消");
    return;
  }
  
  // 3. 左臂运动到start_left_pose
  feedback->progress_percentage = 40.0;
  feedback->current_status = "左臂移动到start_left_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置左臂目标姿态为start_left_pose");
  
  move_group_left_->setNamedTarget("start_left_pose");
  move_group_left_->setStartStateToCurrentState();
  
  moveit::planning_interface::MoveGroupInterface::Plan left_start_plan;
  bool left_start_success = (move_group_left_->plan(left_start_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!left_start_success) {
    result->success = false;
    result->message = "动作类型3失败 - 左臂到start_left_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到start_left_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂到start_left_pose路径规划成功");
  
  feedback->progress_percentage = 50.0;
  feedback->current_status = "左臂返回起始位置...";
  goal_handle->publish_feedback(feedback);
  
  auto left_start_execute_result = move_group_left_->execute(left_start_plan);
  if (left_start_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型3失败 - 左臂到start_left_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "左臂到start_left_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "左臂已到达start_left_pose，左臂操作完成");
  
  // 左臂执行完成后，等待1秒
  feedback->progress_percentage = 55.0;
  feedback->current_status = "等待1秒让机械臂稳定...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "左臂执行完成，等待1秒...");
  std::this_thread::sleep_for(std::chrono::seconds(1));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型3已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型3已取消");
    return;
  }
  
  // ========== 右臂操作流程 ==========
  
  // 先更新机器人状态（包括左臂执行后的所有关节状态）
  RCLCPP_INFO(logger_, "更新右臂起始状态为当前状态...");
  move_group_right_->setStartStateToCurrentState();
  
  // 0. 先将yuntai1运动到0.00
  feedback->progress_percentage = 55.0;
  feedback->current_status = "将yuntai1移动到初始位置...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "将yuntai1设置为0.00");
  
  move_group_right_->setStartStateToCurrentState();
  
  // 获取当前状态并修改yuntai1关节
  moveit::core::RobotStatePtr current_state_right = move_group_right_->getCurrentState(10.0);
  if (!current_state_right) {
    RCLCPP_ERROR(logger_, "无法获取当前机器人状态");
    result->success = false;
    result->message = "动作类型3失败 - 无法获取当前状态";
    goal_handle->abort(result);
    return;
  }
  
  // 获取所有关节的当前值
  std::vector<double> joint_group_positions_right;
  const moveit::core::JointModelGroup* joint_model_group_right = 
    current_state_right->getJointModelGroup("arm_right");
  current_state_right->copyJointGroupPositions(joint_model_group_right, joint_group_positions_right);
  
  // 只修改yuntai1的值
  const std::vector<std::string>& joint_names_right = joint_model_group_right->getVariableNames();
  for (size_t i = 0; i < joint_names_right.size(); i++) {
    if (joint_names_right[i] == "yuntai1") {
      joint_group_positions_right[i] = 0.00;
      RCLCPP_INFO(logger_, "找到yuntai1关节，设置为0.00");
      break;
    }
  }
  
  move_group_right_->setJointValueTarget(joint_group_positions_right);
  
  moveit::planning_interface::MoveGroupInterface::Plan yuntai1_plan;
  bool yuntai1_success = (move_group_right_->plan(yuntai1_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!yuntai1_success) {
    result->success = false;
    result->message = "动作类型3失败 - yuntai1路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "yuntai1路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "yuntai1路径规划成功");
  
  auto yuntai1_execute_result = move_group_right_->execute(yuntai1_plan);
  if (yuntai1_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型3失败 - yuntai1运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "yuntai1运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "yuntai1已到达初始位置");
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型3已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型3已取消");
    return;
  }
  
  // 1. 右臂运动到right_gain_pose
  feedback->progress_percentage = 60.0;
  feedback->current_status = "右臂移动到right_gain_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置右臂目标姿态为right_gain_pose");
  
  move_group_right_->setNamedTarget("right_gain_pose");
  move_group_right_->setStartStateToCurrentState();
  
  moveit::planning_interface::MoveGroupInterface::Plan right_gain_plan;
  bool right_gain_success = (move_group_right_->plan(right_gain_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!right_gain_success) {
    result->success = false;
    result->message = "动作类型3失败 - 右臂到right_gain_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到right_gain_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂到right_gain_pose路径规划成功");
  
  feedback->progress_percentage = 70.0;
  feedback->current_status = "右臂正在移动...";
  goal_handle->publish_feedback(feedback);
  
  auto right_gain_execute_result = move_group_right_->execute(right_gain_plan);
  if (right_gain_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型3失败 - 右臂到right_gain_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到right_gain_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂已到达right_gain_pose");
  
  // 延时1秒
  RCLCPP_INFO(logger_, "到达right_gain_pose后延时1秒...");
  std::this_thread::sleep_for(std::chrono::seconds(1));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型3已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型3已取消");
    return;
  }
  
  // 2. 发布/zhuazi_right为120
  feedback->progress_percentage = 80.0;
  feedback->current_status = "打开右爪子...";
  goal_handle->publish_feedback(feedback);
  
  auto right_msg = std_msgs::msg::Int32();
  right_msg.data = 120;
  zhuazi_right_publisher_->publish(right_msg);
  RCLCPP_INFO(logger_, "发布到 /zhuazi_right: 120 (打开)");
  
  // 等待爪子动作完成
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
  
  if (goal_handle->is_canceling()) {
    result->success = false;
    result->message = "动作类型3已取消";
    goal_handle->canceled(result);
    RCLCPP_INFO(logger_, "动作类型3已取消");
    return;
  }
  
  // 3. 右臂运动到start_right_pose
  feedback->progress_percentage = 90.0;
  feedback->current_status = "右臂移动到start_right_pose...";
  goal_handle->publish_feedback(feedback);
  RCLCPP_INFO(logger_, "设置右臂目标姿态为start_right_pose");
  
  move_group_right_->setNamedTarget("start_right_pose");
  move_group_right_->setStartStateToCurrentState();
  
  moveit::planning_interface::MoveGroupInterface::Plan right_start_plan;
  bool right_start_success = (move_group_right_->plan(right_start_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  
  if (!right_start_success) {
    result->success = false;
    result->message = "动作类型3失败 - 右臂到start_right_pose路径规划失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到start_right_pose路径规划失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂到start_right_pose路径规划成功");
  
  feedback->progress_percentage = 95.0;
  feedback->current_status = "右臂返回起始位置...";
  goal_handle->publish_feedback(feedback);
  
  auto right_start_execute_result = move_group_right_->execute(right_start_plan);
  if (right_start_execute_result != moveit::core::MoveItErrorCode::SUCCESS) {
    result->success = false;
    result->message = "动作类型3失败 - 右臂到start_right_pose运动执行失败";
    goal_handle->abort(result);
    RCLCPP_ERROR(logger_, "右臂到start_right_pose运动执行失败！");
    return;
  }
  
  RCLCPP_INFO(logger_, "右臂已到达start_right_pose，右臂操作完成");
  
  // 动作完成
  feedback->progress_percentage = 100.0;
  feedback->current_status = "放置物体任务完成";
  goal_handle->publish_feedback(feedback);
  
  result->success = true;
  result->message = "动作类型3执行成功 - 左右臂已完成放置物体流程";
  goal_handle->succeed(result);
  RCLCPP_INFO(logger_, "动作类型3执行成功 - 左右臂已完成放置物体流程");
}

} // namespace arm_control

